﻿#ifndef MATH_LIB_H
#define MATH_LIB_H

#include <math.h>

#define PI 3.14159265358979323846f


struct Matrix3x4
{
	float mat34[3][4];
};

struct Matrix3_align16
{
	float mat34[3][4];
	Matrix3_align16(){}
	Matrix3_align16(float *mat3ptr)
	{
		memcpy(mat34[0], mat3ptr, 12);
		memcpy(mat34[1], mat3ptr + 3, 12);
		memcpy(mat34[2], mat3ptr + 6, 12);
		mat34[0][3] = mat34[1][3] = mat34[2][3] = 1.f;
	}
};

struct Vector2
{
	float x, y;
};

struct Vector3
{
	float x, y, z;
};

inline float Dot(const float x[3], const float y[3])
{
	return x[0] * y[0] + x[1] * y[1] + x[2] * y[2];
}

inline void Cross(float result[3], const float x[3], const float y[3])
{
	result[0] = x[1] * y[2] - x[2] * y[1];
	result[1] = x[2] * y[0] - x[0] * y[2];
	result[2] = x[0] * y[1] - x[1] * y[0];
}

inline void Add(float result[3], const float x[3], const float y[3])
{
	result[0] = x[0] + y[0];
	result[1] = x[1] + y[1];
	result[2] = x[2] + y[2];
}

inline void Add(float x[3], const float y[3])
{
	x[0] += y[0];
	x[1] += y[1];
	x[2] += y[2];
}

inline void VectorAdd(float *inOut, float *in2, int count)
{
	for (int i = 0; i < count; i++, inOut++, in2++) *inOut += *in2;
}

inline void VectorCopy(float *dest, float *src, int count)
{
	for (int i = 0; i < count; i++, dest++, src++) *dest = *src;
}

inline void Sub(float result[3], const float x[3], const float y[3])
{
	result[0] = x[0] - y[0];
	result[1] = x[1] - y[1];
	result[2] = x[2] - y[2];
}

inline void Zero(float x[3])
{
	x[0] = 0;
	x[1] = 0;
	x[2] = 0;
}

inline void Set(float x[3], const float y[3])
{
	x[0] = y[0];
	x[1] = y[1];
	x[2] = y[2];
}

inline float Magnitude(const float x[3])
{
	return (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]));
}

inline void Normalize(float x[3])
{
	float invMag = 1 / Magnitude(x);
	x[0] *= invMag;
	x[1] *= invMag;
	x[2] *= invMag;
}

inline void QuatFromEulers(float quat[4], const float vec[3])
{
	float dSY = sin(vec[2] * 0.5f);
	float dSP = sin(vec[1] * 0.5f);
	float dSR = sin(vec[0] * 0.5f);
	float dCY = cos(vec[2] * 0.5f);
	float dCP = cos(vec[1] * 0.5f);
	float dCR = cos(vec[0] * 0.5f);

	quat[0] = dSR * dCP * dCY - dCR * dSP * dSY;
	quat[1] = dCR * dSP * dCY + dSR * dCP * dSY;
	quat[2] = dCR * dCP * dSY - dSR * dSP * dCY;
	quat[3] = dCR * dCP * dCY + dSR * dSP * dSY;
}

inline void QuatFromAxisAngle(float quat[4], const float axis[3], float angle)
{
	float temp[3];
	Set(temp, axis);
	Normalize(temp);
	float sa = sin(angle * 0.5f);
	float ca = cos(angle * 0.5f);
	quat[0] = temp[0] * sa;
	quat[1] = temp[1] * sa;
	quat[2] = temp[2] * sa;
	quat[3] = ca;
}

inline void Lerp(float result[3], const float curr[3], const float next[3], float interpTime)
{
	result[0] = curr[0] + (next[0] - curr[0]) * interpTime;
	result[1] = curr[1] + (next[1] - curr[1]) * interpTime;
	result[2] = curr[2] + (next[2] - curr[2]) * interpTime;
}

void Slerp(float qt[4], const float p[4], float q[4], float t);

void QuaternionMatrix(float matrix[3][4], const float quat[4]);

void R_ConcatTransforms(float out[3][4], const float in1[3][4], const float in2[3][4]);

inline void VectorRotate(float out[3], const float in1[3], const float in2[3][4])
{
	out[0] = Dot(in1, in2[0]);
	out[1] = Dot(in1, in2[1]);
	out[2] = Dot(in1, in2[2]);
}

// rotate by the inverse of the matrix
inline void VectorIRotate(float out[3], const float in1[3], const float in2[3][4])
{
	out[0] = in1[0] * in2[0][0] + in1[1] * in2[1][0] + in1[2] * in2[2][0];
	out[1] = in1[0] * in2[0][1] + in1[1] * in2[1][1] + in1[2] * in2[2][1];
	out[2] = in1[0] * in2[0][2] + in1[1] * in2[1][2] + in1[2] * in2[2][2];
}

inline void VectorTransform(float out[3], const float in1[3], const float in2[3][4])
{
	out[0] = Dot(in1, in2[0]) + in2[0][3];
	out[1] = Dot(in1, in2[1]) + in2[1][3];
	out[2] = Dot(in1, in2[2]) + in2[2][3];
}

inline void VectorITransform(float out[3], const float in1[3], const float in2[3][4])
{
	float temp[3];
	temp[0] = in1[0] - in2[0][3];
	temp[1] = in1[1] - in2[1][3];
	temp[2] = in1[2] - in2[2][3];
	VectorIRotate(out, temp, in2);
}

void MatrixMulV3(float out[3], const float in1[3], const float in2[3][4]);
void QuaternionMultiply(float out[4], const float in1[4], const float in2[4]);
void MuBmdQuaternionFromRotate(float out[4], const float vRotate[3]);
void MuBmdQuaternionRotateMatrix(float matrix[3][4], const float quat[4]);

inline glm::vec4 f32_rgba_cast(int color)
{
    glm::vec4 result;
    result.x = static_cast<float>((color >> 0) & 0xFF) / static_cast<float>(255);
    result.y = static_cast<float>((color >> 8) & 0xFF) / static_cast<float>(255);
    result.z = static_cast<float>((color >> 16) & 0xFF) / static_cast<float>(255);
    result.w = static_cast<float>((color >> 24) & 0xFF) / static_cast<float>(255);
    return result;
}

#endif
